﻿#region Using Statements
using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using starLiGHT.Collections;
using System.Diagnostics;

#endregion

#region COPYRIGHT

/*
    Copyright (c) 2008, 2009, 2010 
        Roland Rosenkranz (Glatzemann@email.de)
*/

#endregion

#region LICENSE

/*
    This file is part of starLiGHT.Collision.

    starLiGHT.Collision is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    starLiGHT.Collision is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with starLiGHT.Collision.  If not, see <http://www.gnu.org/licenses/>.

    
    ADDITIONAL (commercial) LICENSES for starLiGHT.Collision are available on request.
*/

#endregion

#region Box2D Copyright
/*
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
#endregion

#region Version Stuff
// **************[ starLiGHT.Engine SVN ]**********************
// * $Rev:: 1486                  $:  Revision of last commit *
// * $Author:: glatzemann         $:  Author of last commit   *
// * $Date:: 2010-10-08 15:55:47 #$:  Date of last commit     *
// ************************************************************

// **************[ Box2D-Repository Info ]*********************
// Header-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Dynamics/b2World.h
// Revision   : r150
// Change-Date: 2011-03-07
//
// Source-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Dynamics/b2World.cpp
// Revision   : r142
// Change-Date: 2011-02-03
//
// Status     : Finished! Documentation missing.
#endregion

namespace starLiGHT.Physic.TwoD
{
    /// <summary>
    /// Profiling data. Times are in milliseconds.
    /// </summary>
    public struct Profile
    {
        public double step;
        public double collide;
        public double solve;
        public double solveTOI;
    }

    public struct TimeStep
    {
        public float dt;			// time step
        public float inv_dt;		// inverse time step (0 if dt == 0).
        public float dtRatio;	    // dt * inv_dt0
        public int velocityIterations;
        public int positionIterations;
        public bool warmStarting;
    };

    public class WorldQueryWrapper
    {
        public virtual bool QueryCallback(Int32 proxyId)
        {
            FixtureProxy proxy = (FixtureProxy)broadPhase.GetUserData(proxyId);
            return callback.ReportFixture(proxy.fixture);
        }

        public BroadPhase broadPhase;
        public QueryCallback callback;
    };

    /// <summary>The world class manages all physics entities, dynamic simulation, 
    /// and asynchronous queries. The world also contains efficient memory 
    /// management facilities. </summary>
    public class World
    {
        public enum WorldFlags
        {
            e_newFixture = 0x0001,
            e_locked = 0x0002,
            e_clearForces = 0x0004,
        }

        public World(Vector2 gravity, bool doSleep)
        {
            m_destructionListener = null;
            m_debugDraw = null;

            m_bodyList = null;
            m_jointList = null;

            m_bodyCount = 0;
            m_jointCount = 0;

            m_warmStarting = true;
            m_continuousPhysics = true;
            m_subStepping = false;

            m_stepComplete = true;

            m_allowSleep = doSleep;
            m_gravity = gravity;

            m_flags = WorldFlags.e_clearForces;

            m_inv_dt0 = 0.0f;

            m_contactManager = new ContactManager();

            m_rayCastCallbackWrapper = RayCastCallbackWrapper;
        }

        ~World()
        {
            Body b = m_bodyList;
            while (b != null)
            {
                Body bNext = b.m_next;

                Fixture f = b.m_fixtureList;
                while (f != null)
                {
                    Fixture fNext = f.m_next;
                    f.m_proxyCount = 0;
                    f.Destroy();
                    f = fNext;
                }
                b = bNext;
            }
        }

        /// <summary>Register a destruction listener. The listener is owned by you and must remain in scope.</summary>
        public void SetDestructionListener(DestructionListener listener)
        {
            m_destructionListener = listener;
        }

        /// <summary>Register a contact filter to provide specific control over collision.
        /// Otherwise the default filter is used (b2_defaultFilter).
        /// The listener is owned by you and must remain in scope.</summary>
        public void SetContactFilter(ContactFilter filter)
        {
            m_contactManager.m_contactFilter = filter;
        }

        /// <summary>Register a contact event listener. The listener is owned by you and must remain in scope.</summary>
        public void SetContactListener(ContactListener listener)
        {
            m_contactManager.m_contactListener = listener;
        }

        /// <summary>Register a routine for debug drawing. The debug draw functions are called
        /// inside with b2World::DrawDebugData method. The debug draw object is owned
        /// by you and must remain in scope.
        /// </summary>
        public void SetDebugDraw(DebugDraw debugDraw)
        {
            m_debugDraw = debugDraw;
        }

        public Body CreateBody(BodyDef def)
        {
            System.Diagnostics.Debug.Assert(IsLocked() == false);
            if (IsLocked())
            {
                return null;
            }

            Body b = new Body(def, this);

            // Add to world doubly linked list.
            b.m_prev = null;
            b.m_next = m_bodyList;
            if (m_bodyList != null)
            {
                m_bodyList.m_prev = b;
            }
            m_bodyList = b;
            ++m_bodyCount;

            return b;
        }

        /// <summary>Destroy a rigid body given a definition. No reference to the definition
        /// s retained. This function is locked during callbacks.</summary>
        /// <remarks>
        /// This automatically deletes all associated shapes and joints.<br/>
        /// This function is locked during callbacks.<br/>
        /// </remarks>
        public void DestroyBody(Body body)
        {
            System.Diagnostics.Debug.Assert(m_bodyCount > 0);
            System.Diagnostics.Debug.Assert(IsLocked() == false);
            if (IsLocked())
            {
                return;
            }

            // Delete the attached joints.
            JointEdge je = body.m_jointList;
            while (je != null)
            {
                JointEdge je0 = je;
                je = je.next;

                if (m_destructionListener != null)
                {
                    m_destructionListener.SayGoodbye(je0.joint);
                }

                DestroyJoint(je0.joint);

                body.m_jointList = je;
            }
            body.m_jointList = null;

            // Delete the attached contacts
            ContactEdge ce = body.m_contactList;
            while (ce != null)
            {
                ContactEdge ce0 = ce;
                ce = ce.next;
                m_contactManager.Destroy(ce0.contact);
            }
            body.m_contactList = null;

            // Delete the attached fixtures. This destroys broad-phase proxies
            Fixture f = body.m_fixtureList;
            while (f != null)
            {
                Fixture f0 = f;
                f = f.m_next;

                if (m_destructionListener != null)
                {
                    m_destructionListener.SayGoodbye(f0);
                }

                f0.DestroyProxies(m_contactManager.m_broadPhase);
                f0.Destroy();
                f0 = null; ////////TODO: check, was f0.~b2Fixture(); m_blockAllocator.Free(f0, sizeof(b2Fixture));

                body.m_fixtureList = f;
                body.m_fixtureCount -= 1;
            }
            body.m_fixtureList = null;
            body.m_fixtureCount = 0;

            // Remove world body list.
            if (body.m_prev != null)
            {
                body.m_prev.m_next = body.m_next;
            }

            if (body.m_next != null)
            {
                body.m_next.m_prev = body.m_prev;
            }

            if (body == m_bodyList)
            {
                m_bodyList = body.m_next;
            }

            --m_bodyCount;
            //body = null; ////////TODO: check, was b.~Body(); m_blockAllocator.Free(b, sizeof(Body));
        }

        /// <summary>Create a joint to constrain bodies together. No reference to the definition
        /// is retained. This may cause the connected bodies to cease colliding.</summary>
        /// <remarks>This function is locked during callbacks.</remarks>
        public Joint CreateJoint(JointDef def)
        {
            System.Diagnostics.Debug.Assert(IsLocked() == false);
            if (IsLocked()) return null;

            Joint j = Joint.Create(def);

            // Connect to the world list.
            j.m_prev = null;
            j.m_next = m_jointList;
            if (m_jointList != null)
            {
                m_jointList.m_prev = j;
            }
            m_jointList = j;
            ++m_jointCount;

            // Connect to the bodies' doubly linked lists.
            j.m_edgeA.joint = j;
            j.m_edgeA.other = j.m_body2;
            j.m_edgeA.prev = null;
            j.m_edgeA.next = j.m_body1.m_jointList;
            if (j.m_body1.m_jointList != null) j.m_body1.m_jointList.prev = j.m_edgeA;
            j.m_body1.m_jointList = j.m_edgeA;

            j.m_edgeB.joint = j;
            j.m_edgeB.other = j.m_body1;
            j.m_edgeB.prev = null;
            j.m_edgeB.next = j.m_body2.m_jointList;
            if (j.m_body2.m_jointList != null) j.m_body2.m_jointList.prev = j.m_edgeB;
            j.m_body2.m_jointList = j.m_edgeB;

            Body bodyA = def.body1;
            Body bodyB = def.body2;

            // If the joint prevents collisions, then reset collision filtering.
            if (def.collideConnected == false)
            {
                ContactEdge edge = bodyB.ContactList;
                while (edge != null)
                {
                    if (edge.other == bodyA)
                    {
                        // Flag the contact for filtering at the next time step (where either
                        // body is awake).
                        edge.contact.FlagForFiltering();
                    }
                    edge = edge.next;
                }
            }

            // Note: creating a joint doesn't wake the bodies.

            return j;
        }

        /// <summary>Destroy a joint. This may cause the connected bodies to begin colliding.</summary>
        /// <remarks>This function is locked during callbacks.</remarks>
        public void DestroyJoint(Joint j)
        {
            System.Diagnostics.Debug.Assert(IsLocked() == false);
            if (IsLocked()) return;

            bool collideConnected = j.m_collideConnected;

            // Remove from the doubly linked list.
            if (j.m_prev != null)
            {
                j.m_prev.m_next = j.m_next;
            }

            if (j.m_next != null)
            {
                j.m_next.m_prev = j.m_prev;
            }

            if (j == m_jointList)
            {
                m_jointList = j.m_next;
            }

            // Disconnect from island graph.
            Body body1 = j.m_body1;
            Body body2 = j.m_body2;

            // Wake up connected bodies.
            body1.Awake = true;
            body2.Awake = true;

            // Remove from body 1.
            if (j.m_edgeA.prev != null)
            {
                j.m_edgeA.prev.next = j.m_edgeA.next;
            }

            if (j.m_edgeA.next != null)
            {
                j.m_edgeA.next.prev = j.m_edgeA.prev;
            }

            if (j.m_edgeA == body1.m_jointList)
            {
                body1.m_jointList = j.m_edgeA.next;
            }

            j.m_edgeA.prev = null;
            j.m_edgeA.next = null;

            // Remove from body 2
            if (j.m_edgeB.prev != null)
            {
                j.m_edgeB.prev.next = j.m_edgeB.next;
            }

            if (j.m_edgeB.next != null)
            {
                j.m_edgeB.next.prev = j.m_edgeB.prev;
            }

            if (j.m_edgeB == body2.m_jointList)
            {
                body2.m_jointList = j.m_edgeB.next;
            }

            j.m_edgeB.prev = null;
            j.m_edgeB.next = null;

            Joint.Destroy(j);

            System.Diagnostics.Debug.Assert(m_jointCount > 0);
            --m_jointCount;

            // If the joint prevents collisions, then reset collision filtering.
            if (collideConnected == false)
            {
                ContactEdge edge = body2.ContactList;
                while (edge != null)
                {
                    if (edge.other == body1)
                    {
                        // Flag the contact for filtering at the next time step (where either
                        // body is awake).
                        edge.contact.FlagForFiltering();
                    }

                    edge = edge.next;
                }
            }
        }

        /// <summary>Take a time step. This performs collision detection, integration,
        /// and constraint solution.</summary>
        /// <param name="timeStep">the amount of time to simulate, this should not vary.</param>
        /// <param name="velocityIterations">for the velocity constraint solver.</param>
        /// <param name="positionIterations">for the position constraint solver.</param>
        public void Step(float timeStep, int velocityIterations, int positionIterations)
        {
            Stopwatch stepTimer = Stopwatch.StartNew();
            Stopwatch timer = new Stopwatch();

            // If new fixtures were added, we need to find the new contacts.
            if ((m_flags & WorldFlags.e_newFixture) == WorldFlags.e_newFixture)
            {
                m_contactManager.FindNewContacts();
                m_flags &= ~WorldFlags.e_newFixture;
            }

            m_flags |= WorldFlags.e_locked;
            if (WorldLock != null)
                WorldLock(this, null);

            TimeStep step;
            step.dt = timeStep;
            step.velocityIterations = velocityIterations;
            step.positionIterations = positionIterations;
            if (timeStep > 0.0f)
            {
                step.inv_dt = 1.0f / timeStep;
            }
            else
            {
                step.inv_dt = 0.0f;
            }

            step.dtRatio = m_inv_dt0 * timeStep;

            step.warmStarting = m_warmStarting;

            if (PreSolve != null)
                PreSolve(this, step);

            // Update contacts. This is where some contacts are destroyed.
            {
                timer.Start();
                m_contactManager.Collide();
                timer.Stop();
                m_profile.collide = timer.Elapsed.TotalMilliseconds;
                timer.Reset();
            }

            // Integrate velocities, solve velocity constraints, and integrate positions.
            if (m_stepComplete && step.dt > 0.0f)
            {
                timer.Start();
                Solve(step);
                timer.Stop();
                m_profile.solve = timer.Elapsed.TotalMilliseconds;
                timer.Reset();
            }

            // Handle TOI events.
            if (m_continuousPhysics && step.dt > 0.0f)
            {
                timer.Start();
                SolveTOI(step);
                timer.Stop();
                m_profile.solveTOI = timer.ElapsedMilliseconds;
                timer.Reset();
            }

            if (step.dt > 0.0f)
            {
                m_inv_dt0 = step.inv_dt;
            }

            if (PostSolve != null)
                PostSolve(this, step);

            if ((m_flags & WorldFlags.e_clearForces) == WorldFlags.e_clearForces)
            {
                ClearForces();
            }

            m_flags &= ~WorldFlags.e_locked;

            stepTimer.Stop();
            m_profile.step = stepTimer.Elapsed.TotalMilliseconds;

            if (WorldUnlock != null)
                WorldUnlock(this, null);
        }

        /// <summary>
        /// Call this after you are done with time steps to clear the forces. You normally
        /// call this after each call to Step, unless you are perfoming sub-steps. By default,
        /// forces will be automatically cleared, so you don't need to call this function.
        /// <see>SetAutoClearForces</see>
        /// </summary>
        public void ClearForces()
        {
            for (Body body = m_bodyList; body != null; body = body.Next)
            {
                body.m_force = Vector2.Zero;
                body.m_torque = 0.0f;
            }
        }

        public DebugDraw DebugDraw
        {
            get { return m_debugDraw; }
        }

        public void DrawDebugData()
        {
            if (m_debugDraw == null)
            {
                return;
            }

            m_debugDraw.BeginFrame();

            DebugDraw.DrawFlags flags = m_debugDraw.Flags;

            if ((flags & DebugDraw.DrawFlags.Shape) != 0)
            {
                for (Body b = m_bodyList; b != null; b = b.Next)
                {
                    Transform xf = b.Transform;
                    for (Fixture f = b.FixtureList; f != null; f = f.GetNext())
                    {
                        if (b.Active == false)
                        {
                            DrawShape(f, xf, new Color(0.5f, 0.5f, 0.3f));
                        }
                        else if (b.Type == BodyType.STATIC)
                        {
                            DrawShape(f, xf, new Color(0.5f, 0.9f, 0.5f));
                        }
                        else if (b.Type == BodyType.KINEMATIC)
                        {
                            DrawShape(f, xf, new Color(0.5f, 0.5f, 0.9f));
                        }
                        else if (b.Awake == false)
                        {
                            DrawShape(f, xf, new Color(0.6f, 0.6f, 0.6f));
                        }
                        else
                        {
                            DrawShape(f, xf, new Color(0.9f, 0.7f, 0.7f));
                        }
                    }
                }
            }

            if ((flags & DebugDraw.DrawFlags.Joint) == DebugDraw.DrawFlags.Joint)
            {
                for (Joint j = m_jointList; j != null; j = j.GetNext())
                {
                    if (j.GetType() != JointType.MOUSE)
                    {
                        DrawJoint(j);
                    }
                }
            }

            if ((flags & DebugDraw.DrawFlags.Pair) == DebugDraw.DrawFlags.Pair)
            {
                Color color = new Color(0.3f, 0.9f, 0.9f);
                for (Contact c = m_contactManager.m_contactList; c != null; c = c.GetNext())
                {
                    //Fixture fixtureA = c.GetFixtureA();
                    //Fixture fixtureB = c.GetFixtureB();

                    //Vector2 cA = fixtureA.GetAABB().GetCenter();
                    //Vector2 cB = fixtureB.GetAABB().GetCenter();

                    //m_debugDraw.DrawSegment(cA, cB, color);
                }
            }

            if ((flags & DebugDraw.DrawFlags.Aabb) == DebugDraw.DrawFlags.Aabb)
            {
                Color color = new Color(0.9f, 0.3f, 0.9f);
                BroadPhase bp = m_contactManager.m_broadPhase;

                for (Body b = m_bodyList; b != null; b = b.Next)
                {
                    if (b.Active == false)
                    {
                        continue;
                    }

                    for (Fixture f = b.FixtureList; f != null; f = f.GetNext())
                    {
                        for (int i = 0; i < f.m_proxyCount; ++i)
                        {
                            FixtureProxy proxy = f.m_proxies[i];
                            AABB aabb = bp.GetFatAABB(proxy.proxyId);
                            Vector2[] vs = new Vector2[4];
                            vs[0] = new Vector2(aabb.lowerBound.X, aabb.lowerBound.Y);
                            vs[1] = new Vector2(aabb.upperBound.X, aabb.lowerBound.Y);
                            vs[2] = new Vector2(aabb.upperBound.X, aabb.upperBound.Y);
                            vs[3] = new Vector2(aabb.lowerBound.X, aabb.upperBound.Y);

                            m_debugDraw.DrawPolygon(vs, 4, color);
                        }
                    }
                }
            }

            if ((flags & DebugDraw.DrawFlags.CenterOfMass) == DebugDraw.DrawFlags.CenterOfMass)
            {
                for (Body b = m_bodyList; b != null; b = b.Next)
                {
                    Transform xf = b.Transform;
                    xf.position = b.WorldCenter;
                    m_debugDraw.DrawTransform(xf);
                }
            }

            m_debugDraw.EndFrame();
        }

        public void QueryAABB(QueryCallback callback, ref AABB aabb)
        {
            WorldQueryWrapper wrapper = new WorldQueryWrapper();
            wrapper.broadPhase = m_contactManager.m_broadPhase;
            wrapper.callback = callback;
            m_contactManager.m_broadPhase.Query(wrapper, ref aabb);
        }

        public void RayCast(RayCastCallback callback, Vector2 point1, Vector2 point2)
        {
            RayCastInput input = new RayCastInput();
            input.maxFraction = 1.0f;
            input.p1 = point1;
            input.p2 = point2;

            m_rayCastCallback = callback;
            m_contactManager.m_broadPhase.RayCast(m_rayCastCallbackWrapper, ref input);
            m_rayCastCallback = null;
        }

        float RayCastCallbackWrapper(ref RayCastInput input, int proxyId)
        {
            object userData = m_contactManager.m_broadPhase.GetUserData(proxyId);
            FixtureProxy proxy = (FixtureProxy)userData;
            Fixture fixture = proxy.fixture;
            int index = proxy.childIndex;
            RayCastOutput output;
            bool hit = fixture.RayCast(out output, ref input, index);

            if (hit)
            {
                float fraction = output.fraction;
                Vector2 point = (1.0f - fraction) * input.p1 + fraction * input.p2;
                return m_rayCastCallback(fixture, point, output.normal, fraction);
            }

            return input.maxFraction;
        }

        /// <summary>Get the world body list. With the returned body, use Body::GetNext to get
        /// the next body in the world list. A null body indicates the end of the list.</summary>
        /// <returns>the head of the world body list.</returns>
        public Body GetBodyList()
        {
            return m_bodyList;
        }

        /// <summary>Get the world joint list. With the returned joint, use b2Joint::GetNext to get
        /// the next joint in the world list. A null joint indicates the end of the list.</summary>
        /// <returns>the head of the world joint list.</returns>
        public Joint GetJointList()
        {
            return m_jointList;
        }

        /// <summary>Get the world contact list. With the returned contact, use b2Contact::GetNext to get
        /// the next contact in the world list. A null contact indicates the end of the list.</summary>
        /// <returns>the head of the world contact list.</returns>
        public Contact GetContactList()
        {
            return m_contactManager.m_contactList;
        }

        /// <summary>Enable/disable warm starting. For testing.</summary>
        public void SetWarmStarting(bool flag)
        {
            m_warmStarting = flag;
        }

        /// <summary>Enable/disable continuous physics. For testing.</summary>
        public void SetContinuousPhysics(bool flag)
        {
            m_continuousPhysics = flag;
        }

        /// <summary>Enable/disable single stepped continuous physics. For testing.</summary>
        public void SetSubStepping(bool flag)
        {
            m_subStepping = flag;
        }

        /// <summary>Get the number of broad-phase proxies.</summary>
        public int GetProxyCount()
        {
            return m_contactManager.m_broadPhase.GetProxyCount();
        }

        /// <summary>Get the number of bodies.</summary>
        public int GetBodyCount()
        {
            return m_bodyCount;
        }

        /// <summary>Get the number of joints.</summary>
        public int GetJointCount()
        {
            return m_jointCount;
        }

        /// <summary>Get the number of contacts (each may have 0 or more contact points).</summary>
        public int GetContactCount()
        {
            return m_contactManager.m_contactCount;
        }

        /// <summary>Change the global gravity vector.</summary>
        public void SetGravity(Vector2 gravity)
        {
            m_gravity = gravity;
        }

        /// <summary>Get the global gravity vector.</summary>
        public Vector2 GetGravity()
        {
            return m_gravity;
        }

        public bool IsLocked()
        {
            return (m_flags & WorldFlags.e_locked) == WorldFlags.e_locked;
        }

        /// <summary>
        /// Set flag to control automatic clearing of forces after each time step.
        /// </summary>
        /// <param name="flag"></param>
        public void SetAutoClearForces(bool flag)
        {
            if (flag)
            {
                m_flags |= WorldFlags.e_clearForces;
            }
            else
            {
                m_flags &= ~WorldFlags.e_clearForces;
            }
        }

        /// <summary>
        /// Get the flag that controls automatic clearing of forces after each time step.
        /// </summary>
        /// <returns></returns>
        public bool GetAutoClearForces()
        {
            return ((m_flags & WorldFlags.e_clearForces) == WorldFlags.e_clearForces);
        }

        /// <summary>
        /// Get the contact manager for testing.
        /// </summary>
        public ContactManager GetContactManager()
        {
            return m_contactManager;
        }

        /// <summary>
        /// Get the current profile.
        /// </summary>
        public Profile GetProfile()
        {
            return this.m_profile;
        }

        /// <summary>Find islands, integrate and solve constraints, solve position constraints</summary>
        void Solve(TimeStep step)
        {
            // Size the island for the worst case.
            island.Reset(m_bodyCount, m_contactManager.m_contactCount, m_jointCount, m_contactManager.m_contactListener);

            // Clear all the island flags.
            for (Body b = m_bodyList; b != null; b = b.m_next)
            {
                b.m_flags &= ~Body.BodyFlags.e_islandFlag;
            }
            for (Contact c = m_contactManager.m_contactList; c != null; c = c.m_next)
            {
                c.m_flags &= ~Contact.ContactFlags.e_islandFlag;
            }
            for (Joint j = m_jointList; j != null; j = j.m_next)
            {
                j.m_islandFlag = false;
            }

            // Build and simulate all awake islands.
            int stackSize = m_bodyCount;
            Body[] stack = new Body[stackSize];
            for (Body seed = m_bodyList; seed != null; seed = seed.m_next)
            {
                if ((seed.m_flags & Body.BodyFlags.e_islandFlag) != 0)
                {
                    continue;
                }

                if (seed.Awake == false || seed.Active == false)
                {
                    continue;
                }

                // The seed can be dynamic or kinematic.
                if (seed.Type == BodyType.STATIC)
                {
                    continue;
                }

                // Reset island and stack.
                island.Clear();
                int stackCount = 0;
                stack[stackCount++] = seed;
                seed.m_flags |= Body.BodyFlags.e_islandFlag;

                // Perform a depth first search (DFS) on the constraint graph.
                while (stackCount > 0)
                {
                    // Grab the next body off the stack and add it to the island.
                    Body b = stack[--stackCount];
                    System.Diagnostics.Debug.Assert(b.Active == true);
                    island.Add(b);

                    // Make sure the body is awake.
                    b.Awake = true;

                    // To keep islands as small as possible, we don't
                    // propagate islands across static bodies.
                    if (b.Type == BodyType.STATIC)
                    {
                        continue;
                    }

                    // Search all contacts connected to this body.
                    for (ContactEdge ce = b.m_contactList; ce != null; ce = ce.next)
                    {
                        Contact contact = ce.contact;

                        // Has this contact already been added to an island?
                        if ((contact.m_flags & Contact.ContactFlags.e_islandFlag) == Contact.ContactFlags.e_islandFlag)
                        {
                            continue;
                        }

                        // Is this contact solid and touching?
                        if (contact.IsEnabled() == false || contact.IsTouching() == false)
                        {
                            continue;
                        }

                        // Skip sensors.
                        bool sensorA = contact.m_fixtureA.m_isSensor;
                        bool sensorB = contact.m_fixtureB.m_isSensor;
                        if (sensorA || sensorB)
                        {
                            continue;
                        }

                        island.Add(contact);
                        contact.m_flags |= Contact.ContactFlags.e_islandFlag;

                        Body other = ce.other;

                        // Was the other body already added to this island?
                        if ((other.m_flags & Body.BodyFlags.e_islandFlag) != 0)
                        {
                            continue;
                        }

                        System.Diagnostics.Debug.Assert(stackCount < stackSize);
                        stack[stackCount++] = other;
                        other.m_flags |= Body.BodyFlags.e_islandFlag;
                    }

                    // Search all joints connect to this body.
                    for (JointEdge je = b.m_jointList; je != null; je = je.next)
                    {
                        if (je.joint.m_islandFlag == true)
                        {
                            continue;
                        }

                        Body other = je.other;

                        // Don't simulate joints connected to inactive bodies.
                        if (other.Active == false)
                        {
                            continue;
                        }

                        island.Add(je.joint);
                        je.joint.m_islandFlag = true;

                        if ((other.m_flags & Body.BodyFlags.e_islandFlag) != 0)
                        {
                            continue;
                        }

                        System.Diagnostics.Debug.Assert(stackCount < stackSize);
                        stack[stackCount++] = other;
                        other.m_flags |= Body.BodyFlags.e_islandFlag;
                    }
                }

                island.Solve(step, m_gravity, m_allowSleep);

                // Post solve cleanup.
                for (Int32 i = 0; i < island.m_bodyCount; ++i)
                {
                    // Allow static bodies to participate in other islands.
                    Body b = island.m_bodies[i];
                    if (b.Type == BodyType.STATIC)
                    {
                        b.m_flags &= ~Body.BodyFlags.e_islandFlag;
                    }
                }
            }
            stack = null;

            // Synchronize fixtures, check for out of range bodies.
            for (Body b = m_bodyList; b != null; b = b.Next)
            {
                // If a body was not in an island then it did not move.
                if ((b.m_flags & Body.BodyFlags.e_islandFlag) == 0)
                {
                    continue;
                }

                if (b.Type == BodyType.STATIC)
                {
                    continue;
                }

                // Update fixtures (for broad-phase).
                b.SynchronizeFixtures();
            }

            // Update fluid dynamics
            if (sph != null)
            {
                sph.update(m_gravity, step);
                if (SPHContacts != null)
                {
                    foreach (Contact c in SPHContacts.Values)
                    {
                        c.Update(null);
                    }
                }

                // reset fluid forces
                sph.resetForces();
            }

            // Look for new contacts
            m_contactManager.FindNewContacts();
        }

        // Find TOI contacts and solve them.
        void SolveTOI(TimeStep step)
        {
            island.Reset(2 * PhysicSettings.MaxTOIContacts, PhysicSettings.MaxTOIContacts, 0, m_contactManager.m_contactListener);

            if (m_stepComplete)
            {
                for (Body b = m_bodyList; b != null; b = b.m_next)
                {
                    b.m_flags &= ~Body.BodyFlags.e_islandFlag;
                    b.m_sweep.alpha0 = 0.0f;
                }

                for (Contact c = m_contactManager.m_contactList; c != null; c = c.m_next)
                {
                    // Invalidate TOI
                    c.m_flags &= ~(Contact.ContactFlags.e_toiFlag | Contact.ContactFlags.e_islandFlag);
                    c.m_toiCount = 0;
                    c.m_toi = 1.0f;
                }
            }

            // Find TOI events and solve them.
            for (;;)
            {
                // Find the first TOI.
                Contact minContact = null;
                float minAlpha = 1.0f;

                for (Contact c = m_contactManager.m_contactList; c != null; c = c.m_next)
                {
                    // Is this contact disabled?
                    if (c.IsEnabled() == false)
                    {
                        continue;
                    }

                    // Prevent excessive sub-stepping.
                    if (c.m_toiCount > PhysicSettings.MaxSubSteps)
                    {
                        continue;
                    }

                    float alpha = 1.0f;
                    if ((c.m_flags & Contact.ContactFlags.e_toiFlag) == Contact.ContactFlags.e_toiFlag)
                    {
                        // This contact has a valid cached TOI.
                        alpha = c.m_toi;
                    }
                    else
                    {
                        Fixture _fA = c.GetFixtureA();
                        Fixture _fB = c.GetFixtureB();

                        // Is there a sensor?
                        if (_fA.IsSensor() || _fB.IsSensor())
                        {
                            continue;
                        }

                        Body _bA = _fA.GetBody();
                        Body _bB = _fB.GetBody();

                        BodyType typeA = _bA.Type;
                        BodyType typeB = _bB.Type;
                        System.Diagnostics.Debug.Assert(typeA == BodyType.DYNAMIC || typeB == BodyType.DYNAMIC);

                        bool awakeA = _bA.Awake && typeA != BodyType.STATIC;
                        bool awakeB = _bB.Awake && typeB != BodyType.STATIC;

                        // Is at least one body awake?
                        if (awakeA == false && awakeB == false)
                        {
                            continue;
                        }

                        bool collideA = _bA.Bullet || typeA != BodyType.DYNAMIC;
                        bool collideB = _bB.Bullet || typeB != BodyType.DYNAMIC;

                        // Are these two non-bullet dynamic bodies?
                        if (collideA == false && collideB == false)
                        {
                            continue;
                        }

                        // Compute the TOI for this contact.
                        // Put the sweeps onto the same time interval.
                        float alpha0 = _bA.m_sweep.alpha0;

                        if (_bA.m_sweep.alpha0 < _bB.m_sweep.alpha0)
                        {
                            alpha0 = _bB.m_sweep.alpha0;
                            _bA.m_sweep.Advance(alpha0);
                        }
                        else if (_bB.m_sweep.alpha0 < _bA.m_sweep.alpha0)
                        {
                            alpha0 = _bA.m_sweep.alpha0;
                            _bB.m_sweep.Advance(alpha0);
                        }

                        System.Diagnostics.Debug.Assert(alpha0 < 1.0f);

                        int indexA = c.GetChildIndexA();
                        int indexB = c.GetChildIndexB();

                        // Compute the time of impact in interval [0, minTOI]
                        TOIInput input = new TOIInput();
                        input.proxyA.Set(_fA.GetShape(), indexA);
                        input.proxyB.Set(_fB.GetShape(), indexB);
                        input.sweepA = _bA.m_sweep;
                        input.sweepB = _bB.m_sweep;
                        input.tMax = 1.0f;

                        TOIOutput output;
                        Collision.TimeOfImpact(out output, input);

                        // Beta is the fraction of the remaining portion of the .
                        float beta = output.t;
                        if (output.state == TOIOutput.TOIOutputState.e_touching)
                        {
                            alpha = System.Math.Min(alpha0 + (1.0f - alpha0) * beta, 1.0f);
                        }
                        else
                        {
                            alpha = 1.0f;
                        }

                        c.m_toi = alpha;
                        c.m_flags |= Contact.ContactFlags.e_toiFlag;
                    }

                    if (alpha < minAlpha)
                    {
                        // This is the minimum TOI found so far.
                        minContact = c;
                        minAlpha = alpha;
                    }
                }

                if (minContact == null || 1.0f - 10.0f * PhysicSettings.FLT_EPSILON < minAlpha)
                {
                    // No more TOI events. Done!
                    m_stepComplete = true;
                    break;
                }

                // Advance the bodies to the TOI.
                Fixture fA = minContact.GetFixtureA();
                Fixture fB = minContact.GetFixtureB();
                Body bA = fA.GetBody();
                Body bB = fB.GetBody();

                Sweep backup1 = bA.m_sweep;
                Sweep backup2 = bB.m_sweep;

                bA.Advance(minAlpha);
                bB.Advance(minAlpha);

                // The TOI contact likely has some new contact points.
                minContact.Update(m_contactManager.m_contactListener);
                minContact.m_flags &= ~Contact.ContactFlags.e_toiFlag;
                ++minContact.m_toiCount;

                // Is the contact solid?
                if (minContact.IsEnabled() == false || minContact.IsTouching() == false)
                {
                    // Restore the sweeps.
                    minContact.SetEnabled(false);
                    bA.m_sweep = backup1;
                    bB.m_sweep = backup2;
                    bA.SynchronizeTransform();
                    bB.SynchronizeTransform();
                    continue;
                }

                bA.Awake = true;
                bB.Awake = true;

                // Build the island
                island.Clear();
                island.Add(bA);
                island.Add(bB);
                island.Add(minContact);

                bA.m_flags |= Body.BodyFlags.e_islandFlag;
                bB.m_flags |= Body.BodyFlags.e_islandFlag;
                minContact.m_flags |= Contact.ContactFlags.e_islandFlag;

                // Get contacts on bodyA and bodyB.
                Body[] bodies = {bA, bB};
                for (int i = 0; i < 2; ++i)
                {
                    Body body = bodies[i];
                    if (body.m_type == BodyType.DYNAMIC)
                    {
                        for (ContactEdge ce = body.m_contactList; ce != null && island.m_bodyCount < PhysicSettings.MaxTOIContacts; ce = ce.next)
                        {
                            Contact contact = ce.contact;

                            // Has this contact already been added to the island?
                            if ((contact.m_flags & Contact.ContactFlags.e_islandFlag) == Contact.ContactFlags.e_islandFlag)
                            {
                                continue;
                            }

                            // Only add static, kinematic, or bullet bodies.
                            Body other = ce.other;
                            if (other.m_type == BodyType.DYNAMIC &&
                                body.Bullet == false && other.Bullet == false)
                            {
                                continue;
                            }

                            // Skip sensors.
                            bool sensorA = contact.m_fixtureA.m_isSensor;
                            bool sensorB = contact.m_fixtureB.m_isSensor;
                            if (sensorA || sensorB)
                            {
                                continue;
                            }

                            // Tentatively advance the body to the TOI.
                            Sweep backup = other.m_sweep;
                            if ((other.m_flags & Body.BodyFlags.e_islandFlag) == Body.BodyFlags.e_islandFlag)
                            {
                                other.Advance(minAlpha);
                            }

                            // Update the contact points
                            contact.Update(m_contactManager.m_contactListener);

                            // Was the contact disabled by the user?
                            if (contact.IsEnabled() == false)
                            {
                                other.m_sweep = backup;
                                other.SynchronizeTransform();
                                continue;
                            }

                            // Are there contact points?
                            if (contact.IsTouching() == false)
                            {
                                other.m_sweep = backup;
                                other.SynchronizeTransform();
                                continue;
                            }

                            // Add the contact to the island
                            contact.m_flags |= Contact.ContactFlags.e_islandFlag;
                            island.Add(contact);

                            // Has the other body already been added to the island?
                            if ((other.m_flags & Body.BodyFlags.e_islandFlag) == Body.BodyFlags.e_islandFlag)
                            {
                                continue;
                            }

                            // Add the other body to the island.
                            other.m_flags |= Body.BodyFlags.e_islandFlag;

                            if (other.m_type != BodyType.STATIC)
                            {
                                other.Awake = true;
                            }

                            island.Add(other);
                        }
                    }
                }

                TimeStep subStep;
                subStep.dt = (1.0f - minAlpha) * step.dt;
                subStep.inv_dt = 1.0f / subStep.dt;
                subStep.dtRatio = 1.0f;
                subStep.positionIterations = 20;
                subStep.velocityIterations = step.velocityIterations;
                subStep.warmStarting = false;
                island.SolveTOI(subStep, bA, bB);

                // Reset island flags and synchronize broad-phase proxies.
                for (int i = 0; i < island.m_bodyCount; ++i)
                {
                    Body body = island.m_bodies[i];
                    body.m_flags &= ~Body.BodyFlags.e_islandFlag;
                    if (body.m_type != BodyType.DYNAMIC)
                    {
                        continue;
                    }

                    body.SynchronizeFixtures();

                    // Invalidate all contact TOIs on this displaced body.
                    for (ContactEdge ce = body.m_contactList; ce != null; ce = ce.next)
                    {
                        ce.contact.m_flags &= ~(Contact.ContactFlags.e_toiFlag | Contact.ContactFlags.e_islandFlag);
                    }
                }

                // Commit fixture proxy movements to the broad-phase so that new contacts are created.
                // Also, some contacts can be destroyed.
                m_contactManager.FindNewContacts();

                if (m_subStepping)
                {
                    m_stepComplete = false;
                    break;
                }
            }
        }

        private void DrawJoint(Joint joint)
        {
            Body bodyA = joint.GetBodyA();
            Body bodyB = joint.GetBodyB();
            Transform xf1 = bodyA.Transform;
            Transform xf2 = bodyB.Transform;
            Vector2 x1 = xf1.position;
            Vector2 x2 = xf2.position;
            Vector2 p1 = joint.GetAnchorA();
            Vector2 p2 = joint.GetAnchorB();

            Color color = new Color(0.5f, 0.8f, 0.8f);

            switch (joint.GetType())
            {
                case JointType.DISTANCE:
                    m_debugDraw.DrawSegment(p1, p2, color);
                    break;

                case JointType.PULLEY:
                    PulleyJoint pulley = (PulleyJoint)joint;
                    Vector2 s1 = pulley.GetGroundAnchor1();
                    Vector2 s2 = pulley.GetGroundAnchor2();
                    m_debugDraw.DrawSegment(s1, p1, color);
                    m_debugDraw.DrawSegment(s2, p2, color);
                    m_debugDraw.DrawSegment(s1, s2, color);
                    break;

                case JointType.MOUSE:
                    // don't draw this
                    break;

                default:
                    m_debugDraw.DrawSegment(x1, p1, color);
                    m_debugDraw.DrawSegment(p1, p2, color);
                    m_debugDraw.DrawSegment(x2, p2, color);
                    break;
            }
        }

        private void DrawShape(Fixture fixture, Transform xf, Color color)
        {
            switch (fixture.GetType())
            {
                case ShapeType.CIRCLE:
                    {
                        CircleShape circle = (CircleShape)fixture.GetShape();

#if !OPTIMIZE
                        Vector2 center = Math.Mul(xf, circle.m_p);
                        float radius = circle.m_radius;
                        Vector2 axis = xf.R.col1;

                        m_debugDraw.DrawSolidCircle(center, radius, axis, color);
#else
                        Vector2 t; t.X = xf.R.col1.X * circle.m_p.X + xf.R.col2.X * circle.m_p.Y; t.Y = xf.R.col1.Y * circle.m_p.X + xf.R.col2.Y * circle.m_p.Y;
                        Vector2 center = Vector2.Zero; center.X = xf.position.X + t.X; center.Y = xf.position.Y + t.Y;

                        m_debugDraw.DrawSolidCircle(center, circle.m_radius, xf.R.col1, color);
#endif
                    }
                    break;

                case ShapeType.EDGE:
                    {
                        EdgeShape edge = (EdgeShape)fixture.GetShape();
                        Vector2 v1 = Math.Mul(xf, edge.m_vertex1);
                        Vector2 v2 = Math.Mul(xf, edge.m_vertex2);
                        m_debugDraw.DrawSegment(v1, v2, color);
                    }
                    break;

                case ShapeType.LOOP:
                    {
                        LoopShape loop = (LoopShape)fixture.GetShape();
                        int count = loop.Count;
                        Vector2 v1 = Math.Mul(xf, loop.GetVertex(count - 1));
                        for (int i = 0; i < count; ++i)
                        {
                            Vector2 v2 = Math.Mul(xf, loop.GetVertex(i));
                            m_debugDraw.DrawSegment(v1, v2, color);
                            m_debugDraw.DrawCircle(v1, 0.05f, color);
                            v1 = v2;
                        }
                    }
                    break;

                case ShapeType.POLYGON:
                    {
                        PolygonShape poly = (PolygonShape)fixture.GetShape();
                        int vertexCount = poly.m_vertexCount;
                        System.Diagnostics.Debug.Assert(vertexCount <= PhysicSettings.MaxPolygonVertices);
                        Vector2[] vertices = new Vector2[PhysicSettings.MaxPolygonVertices];

#if !OPTIMIZE
                        for (int i = 0; i < vertexCount; ++i)
                        {
                            vertices[i] = Math.Mul(xf, poly.m_vertices[i]);
                        }
#else
                        Vector2 t, pmi;
                        for (int i = 0; i < vertexCount; ++i)
                        {
                            pmi = poly.m_vertices[i];
                            t.X = xf.R.col1.X * pmi.X + xf.R.col2.X * pmi.Y; t.Y = xf.R.col1.Y * pmi.X + xf.R.col2.Y * pmi.Y;
                            vertices[i].X = xf.position.X + t.X;
                            vertices[i].Y = xf.position.Y + t.Y;
                        }

#endif
                        m_debugDraw.DrawSolidPolygon(vertices, vertexCount, color);
                    }
                    break;
            }
        }

        internal WorldFlags m_flags;

        internal ContactManager m_contactManager;

        Body m_bodyList;
        Joint m_jointList;

        int m_bodyCount;
        int m_jointCount;

        Vector2 m_gravity;
        bool m_allowSleep;

        DestructionListener m_destructionListener;
        DebugDraw m_debugDraw;

        // This is used to compute the time step ratio to
        // support a variable time step.
        float m_inv_dt0;

        // These are for debugging the solver.
        private bool m_warmStarting;
        private bool m_continuousPhysics;
        private bool m_subStepping;
        private bool m_stepComplete;

        private Profile m_profile;

        private Island island = new Island();

        RayCastCallback m_rayCastCallback;
        RayCastCallbackInternal m_rayCastCallbackWrapper;

        public event EventHandler WorldLock;
        public event EventHandler WorldUnlock;
        public event PreSolveEventHandler PreSolve;
        public event PreSolveEventHandler PostSolve;

        //////////////////////////////////////////////////////////////
        // SPH Stuff
        /////////////////////////////////////////////////////////////

        internal SPHSimulation sph;
        internal Dictionary<int, Contact> SPHContacts;

        public void addFluidParticle(FluidParticle fixture)
        {
            if (sph == null) sph = new SPHSimulation(this);

            sph.addParticle(fixture);
        }




    }
}
